/*
tc: progressing inputs
*/
#include "control.h"
#include "motion9_dll.h"
#include "./trajectory_plane/fr.h"
#include "./trajectory_plane/tp_type.h"
#include "./trajectory_plane/tp.h"

extern STRUCT_CONTROL Ct;

int Cmd_ParseCommand_CMD_PLANE()
{
	//clear tp error flag
	Ct.tp.tp_have_error = 0;
	if (EM_INTER_PLANE == Ct.inter_mode){
		//DLog("Tro->Done Command->PALNE, And for last mode is PLANE");
		return 0;
	}
	else if (Motion_QueryMoveDone() == 0){
		memcpy(Ct.tp.crp, Ct.curr_posit, sizeof(double)*MAX_JOINT);
		memcpy(Ct.tp.goal_posit, Ct.curr_posit, sizeof(double)*MAX_JOINT);
		Ct.inter_mode = EM_INTER_PLANE;
		DLog("Tro->Done Command->PALNE");
		return 0;
	}
	else{
		Controller_MsgVp(MOTION_WARN_WRITE_CMD_PALNE_FOR_FREE_RUN, "id=-1\n");
		DLog("Tro->Faild Command->PALNE, for FREE is runing");
		return -1;
	}
}

int Cmd_ParseCommand_CMD_FREE()
{
	if (EM_INTER_FREE == Ct.inter_mode){
		//DLog("Tro->Done Command->FREE,And for last mode is FREE");
		return 0;
	}
	else if (Motion_QueryMoveDone() == 0){
		memcpy(Ct.fr.crp, Ct.curr_posit, sizeof(double)*MAX_JOINT);
		Ct.inter_mode = EM_INTER_FREE;
		DLog("Tro->Done Command->FREE");
		return 0;
	}
	else{
		Controller_MsgVp(MOTION_WARN_WRITE_CMD_FREE_FOR_PLANE_RUN, "id=-1\n");
		DLog("Tro->Faild Command->FREE, for Plane is runing");
		return -1;
	}
}

int Cmd_ParseCommand_CMD_ABORT()
{
	if (CMD_STATUS_ABORTING == Ct.cmd_status || CMD_STATUS_MOVING_IDLE == Ct.cmd_status){
		DLog("Tro->Skip Command->ABORT.cmd_status = %d\n", Ct.cmd_status);
	}
	else if (EM_INTER_PLANE == Ct.inter_mode){
		tpHandleAbort(&Ct.tp);
	}
	else if (EM_INTER_FREE == Ct.inter_mode){
		frHandleAbort(&Ct.fr);
	}
	return 0;
}

int Cmd_ParseCommand_CMD_EMERGEMCY()
{
	Ct.cmd_status = CMD_STATUS_EMERGENCY;
	Controller_Clear();
	DLog("Tro->Done Command->Emergency");
	return 0;
}

//release emergency
int Cmd_ParseCommand_CMD_EMERGEMCY_RELEASE()
{
	if (CMD_STATUS_EMERGENCY == Ct.cmd_status){
		Ct.cmd_status = CMD_STATUS_MOVING_IDLE;
		DLog("Tro->Done Command->EmergencyRelease,cmd_status=%d", Ct.cmd_status);
	}
	else{
		DLog("Tro->Skip Command->EmergencyRelease,cmd_status=%d", Ct.cmd_status);
	}
	return 0;
}

int Cmd_ParseCommand_CMD_PAUSE()
{
	if (CMD_STATUS_MOVING_PLANE == Ct.cmd_status){
		tpHandlePause(&Ct.tp);
	}
	else if (CMD_STATUS_MOVING_FREE == Ct.cmd_status){
		frHandlePause(&Ct.fr);
	}
	else if (CMD_STATUS_ABORTING == Ct.cmd_status || CMD_STATUS_MOVING_IDLE == Ct.cmd_status){
		DLog("Tro->Skip Command->PAUSE.cmd_status = %d", Ct.cmd_status);
		return 0;
	}
	return 0;
}

//Cmd resume
int Cmd_ParseCommand_CMD_RESUME()
{
	//DLog("Tro->Progressing Command->RESUME");
	if (CMD_STATUS_MOVING_FREE == Ct.cmd_status || CMD_STATUS_MOVING_PLANE == Ct.cmd_status
		|| CMD_STATUS_PAUSING == Ct.cmd_status || CMD_STATUS_PAUSED == Ct.cmd_status){
		if (EM_INTER_PLANE == Ct.inter_mode){
			tpHandleResume(&Ct.tp);
		}
		else if (EM_INTER_FREE == Ct.inter_mode){
			frHandleResume(&Ct.fr);
		}
		return 0;
	}
	else {
		DLog("Tro->Skip Command->RESUME,cmd_status=%d", Ct.cmd_status);
		return 0;
	}
}

//check velrate
void Cmd_CheckVelRate(float *vel)
{
	if (*vel > TP_MAX_FEED_RATE){
		Controller_MsgVp(MOTION_WARN_CHANGE_VELOCITY_VEL, "id=-1 vel_rate=%f\n", vel);
		*vel = TP_MAX_FEED_RATE;
	}
	else if (*vel < TP_MIN_FEED_RATE){
		Controller_MsgVp(MOTION_WARN_CHANGE_VELOCITY_VEL, "id=-1 vel_rate=%f\n", vel);
		*vel = TP_MIN_FEED_RATE;
	}
}

//change velocity
int Cmd_ParseCommand_CMD_ChangeVel(float vel)
{
	Cmd_CheckVelRate(&vel);
	if (EM_INTER_PLANE == Ct.inter_mode){
		tpHandleChangeVelRate(&Ct.tp, vel);
	}
	else if (EM_INTER_FREE == Ct.inter_mode){
		frHandleChangeVelRate(&Ct.fr, vel);
	}
	else{
		Ct.feed_rate = vel;
		DLog("Tro->Done Command->ChangelVel,Vl=%f", vel);
	}
	return 0;
}